#include "Car_ctrl.h"
#include <math.h>
#include <stdlib.h>
#include "stimer.h"
#include "board_base.h"
#include "bsp_uart.h"
#define debug_print(str, ...) UARTx_Printf(&huart2, str, ##__VA_ARGS__)
#define EVENT_CLEAR(x) (CarCtrl.event &= ~(x))
#define EVENT_SET(x) (CarCtrl.event |= (x))

Car_ctrl_t CarCtrl = {0};

// 初始化
int car_ctrl_init(CarPort_t *port)
{
    pid_inc_init(&CarCtrl.pid_LSpeed, 0.03, 0, 0.01);
    pid_inc_init(&CarCtrl.pid_RSpeed, 0.03, 0, 0.01);
    pid_inc_init(&CarCtrl.pid_Angle, 4, 0, 1);
    pid_inc_init(&CarCtrl.pid_Track, 1.4, 0, 0);
    CarCtrl.port = port;
    CarCtrl.state = CAR_CTRL_STATE_IDLE;
    CarCtrl.task = CAR_CTRL_TASK_IDLE;
    CarCtrl.tracker_event_cmp_time_line = 1000;
    CarCtrl.tracker_event_cmp_time_noline = 1000;

    return 0;
}


// 定速运动
int car_ctrl_start_speed(float speedL, float speedR)
{
    CarCtrl.port->motor_enable(1);
    // 设置目标速度
    CarCtrl.fb_LSpeed = speedL;
    CarCtrl.fb_RSpeed = speedR;
    // 角度闭环 or 速度闭环
    CarCtrl.speed_fb_flag = 1;
    CarCtrl.angle_fb_flag = 0;
    CarCtrl.track_fb_flag = 0;
    CarCtrl.state = CAR_CTRL_STATE_LINE;
    return 0;
}

// 旋转
int car_ctrl_start_rotate(float angle, float speed)
{
    CarCtrl.port->motor_enable(1);
    CarCtrl.fb_angle = angle;
    CarCtrl.fb_angleSpeed = speed;
    // 开启角度闭环
    CarCtrl.angle_fb_flag = 1;
    CarCtrl.speed_fb_flag = 0;
    CarCtrl.track_fb_flag = 0;
    CarCtrl.state = CAR_CTRL_STATE_ROTATE;
    return 0;
}
// 循迹
int car_ctrl_start_track(float speed)
{
    // 循迹模式
    CarCtrl.port->motor_enable(1);
    CarCtrl.fb_trackSpeed = speed;
    // 开启循迹闭环
    CarCtrl.track_fb_flag = 1;
    CarCtrl.speed_fb_flag = 0;
    CarCtrl.angle_fb_flag = 0;
    CarCtrl.state = CAR_CTRL_STATE_TRACK;
    return 0;
}
// 停车
int car_ctrl_stop(void)
{
    // CarCtr
    // CarCtrl.port->motor_enable(0);
    CarCtrl.angle_fb_flag = 0;
    CarCtrl.speed_fb_flag = 0;
    CarCtrl.track_fb_flag = 0;
    CarCtrl.check_point_flag = 0;
    CarCtrl.event = 0;
    CarCtrl.port->left_pwm_write(0);
    CarCtrl.port->right_pwm_write(0);
    CarCtrl.state = CAR_CTRL_STATE_STOP;
    return 0;
}
int Tracker_Haven_Line(uint8_t *dat, uint8_t num)
{
    int i;
    for (i = 0; i < num; i++)
    {
        if (dat[i])
            return 1;
    }
    return 0;
}

//  寻找位置数据,   自右往左找
int8_t Tracker_Find_Position_Right(const uint8_t *dat, int num) 
{
    int l = num, r = num, i, start = 0;
    int8_t position = 0;
    /*
            <---- find
        7654 3210
        0011 1000 right
          ^  ^
          |  |
       end   start
       7 - (3 + 5) = -1
    */
    for (i = 0; i < num; i++) //  自右往左找
    {
        if (start == 1)
        {
            if ((dat[i]) == 0) //  找0
            {
                r = i - 1;
                break;
            }
            if (i == num - 1)
            {
                r = i;
                break;
            }
        }
        if (start == 0 && (dat[i]) != 0) //  找1
        {
            if (i == num - 1) //  最后一个点
            {
                r = l = i; // 说明没有找到，输出0
                break;
            }
            else
            {
                l = i;
                start = 1;
            }
        }
    }

    if (l < num && r < num)
    {
        position = (num - 1) - (l + r);
    }
    return position;
}

//  寻找位置数据,   自左往右找
int8_t Tracker_Find_Position_Left(uint8_t *dat, uint8_t num)
{
    int l = num, r = num, i, start = 0;
    int8_t position = 0;
    /*
    find ----> 
         7654 3210
    left 0011 1000 right
           ^  ^
           |  |
        end   start
        7 - (3 + 5) = -1
    */
    for (i = num-1; i >= 0; i--) // 自左往右找
    {
        if (start == 1)
        {
            if (dat[i] == 0) //  找变0
            {
                r = i + 1;
                break;
            }
            if (i == 0)
            {
                r = i;
                break;
            }
        }
        if (start == 0 && (dat[i]) != 0) //  找变1
        {
            if (i == 0)
            {
                r = l = i; // 没找到返回0
                break;
            }
            else
            {
                l = i;
                start = 1;
            }
        }
    }

    if (l < num && r < num)
    {
        position = (num - 1) - (l + r);
    }
    return position;
}
// 计算循迹板输出
int car_ctrl_track_calculate(void)
{
    const int TRACKER_SIZE = 16;
    int8_t track_Position_r, track_Position_l;
    uint8_t track[TRACKER_SIZE];
    Tracker_t *tracker = &CarCtrl.tracker;
    // 读取循迹板数据
    CarCtrl.tracker.data = CarCtrl.port->tracker_read(&track[0], TRACKER_SIZE);
    //  获取位置偏移
    tracker->errR = Tracker_Find_Position_Right(&track[0], TRACKER_SIZE);
    tracker->errL = Tracker_Find_Position_Left(&track[0], TRACKER_SIZE);
    tracker->position = (int8_t)tracker->errR; //  右开始
    if (abs(tracker->position) <= TRACKER_SIZE - 1)
    {
        track_Position_r = tracker->position;
    }
    tracker->position = (int8_t)tracker->errL; //  左开始
    if (abs(tracker->position) <= TRACKER_SIZE - 1)
    {
        track_Position_l = tracker->position;
    }

    if (track_Position_l != track_Position_r) // 两条线
    {

    }

    tracker->position = track_Position_r;
#if 1 
    if (Tracker_Haven_Line(&track[0], TRACKER_SIZE) == 0) //  无黑线
    {
        tracker->timeNoLine = bsp_MesureTime(&tracker->timeNoLineCnt); //  记录离开黑线的时间
        if (tracker->timeNoLine > CarCtrl.tracker_event_cmp_time_noline)
        {
            CarCtrl.event |= CAR_CTRL_NO_FIND_LINE;
        }
        tracker->noLineCnt++; // 记录离开黑线的次数
        tracker->position = tracker->_position*2;//  用上次的值，放大2倍
    }
    else
    {
        tracker->timeLine = bsp_MesureTime(&tracker->timeLineCnt); //  记录进入黑线的时间
        if (tracker->timeLine > CarCtrl.tracker_event_cmp_time_line)
        {
            CarCtrl.event |= CAR_CTRL_FIND_LINE;
        }
        tracker->LineCnt++; // 记录进入黑线的次数
        tracker->_position = tracker->position;//  保留上次位置值
        if (tracker->position > 4 || tracker->position < -4) // 增益
        {
            tracker->position *= 2;
        }
    }
#endif
    return 0;
}

/**
 * @brief 先调用更新编码器再调用该函数频率100Hz
 */
void car_ctrl_speed_loop(void)
{
    // 单位时间内的脉冲数就是速度
    int encL_real = CarCtrl.port->encoder_read(0);
    int encR_real = CarCtrl.port->encoder_read(1);
    CarCtrl.real_LSpeed = encL_real - CarCtrl._encL;
    CarCtrl.real_RSpeed = encR_real - CarCtrl._encR;
    CarCtrl._encL = encL_real;
    CarCtrl._encR = encR_real;
    if (CarCtrl.speed_fb_flag) // 速度闭环
    {
        CarCtrl.out_LSpeed += pid_inc_calculate(&CarCtrl.pid_LSpeed,
                                                CarCtrl.fb_LSpeed,
                                                CarCtrl.real_LSpeed);
        CarCtrl.out_RSpeed += pid_inc_calculate(&CarCtrl.pid_RSpeed,
                                                CarCtrl.fb_RSpeed,
                                                CarCtrl.real_RSpeed);
        // 执行输出
        if (CarCtrl.out_LSpeed > 100)
            CarCtrl.out_LSpeed = 100;
        if (CarCtrl.out_RSpeed > 100)
            CarCtrl.out_RSpeed = 100;
        if (CarCtrl.out_LSpeed < -100)
            CarCtrl.out_LSpeed = -100;
        if (CarCtrl.out_RSpeed < -100)
            CarCtrl.out_RSpeed = -100;
        CarCtrl.port->left_pwm_write(CarCtrl.out_LSpeed);
        CarCtrl.port->right_pwm_write(CarCtrl.out_RSpeed);
    }
}

/**
 * @brief 先调用角度刷新在调用该函数频率100Hz
 */
void car_ctrl_angle_loop(void)
{
    // 一个往前一个往后就能旋转
    float err_pwm_kp;
    float err_pwm_kd;
    CarCtrl.real_angle = CarCtrl.port->angle_read();
    if (CarCtrl.angle_fb_flag) // 角度闭环
    {
        // 计算PID输出
        // 比例
        err_pwm_kp = (CarCtrl.fb_angle - CarCtrl.real_angle) * CarCtrl.pid_Angle.Kp;
        // 阻尼
        err_pwm_kd = - CarCtrl.port->gyroz_read() * CarCtrl.pid_Angle.Kd;
        CarCtrl.out_RSpeed = CarCtrl.fb_angleSpeed + err_pwm_kp + err_pwm_kd;
        CarCtrl.out_LSpeed = CarCtrl.fb_angleSpeed - err_pwm_kp - err_pwm_kd;
#if 0
        // 限制输出
        if (CarCtrl.out_LSpeed > 50)
            CarCtrl.out_LSpeed = 50;
        if (CarCtrl.out_LSpeed < -50)
            CarCtrl.out_LSpeed = -50;
        if (fabs(CarCtrl.out_angleSpeed) < 4)
            CarCtrl.out_angleSpeed = 0;
#endif
        // 执行输出
        CarCtrl.port->left_pwm_write(CarCtrl.out_LSpeed);
        CarCtrl.port->right_pwm_write(CarCtrl.out_RSpeed);
    }
}

void car_ctrl_set_speed(float speedL, float speedR)
{
    if (CarCtrl.speed_fb_flag)
    {
        CarCtrl.fb_LSpeed = speedL;
        CarCtrl.fb_RSpeed = speedR;
    }
    else
    {
        CarCtrl.port->left_pwm_write(speedL);
        CarCtrl.port->right_pwm_write(speedR);
    }
}

/**
 * @brief 先调用刷新循迹数据和陀螺仪数据再调用该函数
 */
void car_ctrl_track_loop(void)
{
    float err_pwm_kp;
    float err_pwm_kd;
    // 计算PID输出
    car_ctrl_track_calculate();
    if (CarCtrl.track_fb_flag)
    {
        // 比例误差
        err_pwm_kp = CarCtrl.tracker.position * CarCtrl.pid_Track.Kp;
        // 微分抑制
        err_pwm_kd = -CarCtrl.port->gyroz_read() * CarCtrl.pid_Track.Kd;
        // 计算PWM输出
        CarCtrl.out_RSpeed = CarCtrl.fb_trackSpeed - err_pwm_kp - err_pwm_kd;
        CarCtrl.out_LSpeed = CarCtrl.fb_trackSpeed + err_pwm_kp + err_pwm_kd;
        // 输出限制
        if (CarCtrl.out_LSpeed < -20)
            CarCtrl.out_LSpeed = -20;
        if (CarCtrl.out_RSpeed < -20)
            CarCtrl.out_RSpeed = -20;
        // 执行输出
        CarCtrl.port->left_pwm_write(CarCtrl.out_LSpeed);
        CarCtrl.port->right_pwm_write(CarCtrl.out_RSpeed);
    }
    car_ctrl_check_point();
}

void task_stop_move(const void * arg)
{
    car_ctrl_set_speed(0, 0);
    EVENT_SET(CAR_CTRL_TIME_EVENT);
}
/**
 * @brief 旋转一段时间
 * @param time Ms
 * @param speed speed > 0 右转 speed < 0 左转
 */
void car_ctrl_rotate_open(float time, float speed)
{
    CarCtrl.angle_fb_flag = 0;
    CarCtrl.speed_fb_flag = 0;
    CarCtrl.track_fb_flag = 0;
    car_ctrl_set_speed(speed, -speed);
    stimer_task_oneshot(task_stop_move, time, 1, NULL);
}

void car_ctrl_check_point(void)
{
    if (CarCtrl.delayTime > 0) // 延时
    {
        CarCtrl.delayTime--;
        return;
    }
    uint32_t event = CarCtrl.event;
    if (CarCtrl.check_point_flag && event)
    {
        if (event & CAR_CTRL_FIND_LINE) // 离开线一段时间后进入线
        {
            EVENT_CLEAR(CAR_CTRL_FIND_LINE);
            debug_print("Find Line : %d \r\n", CarCtrl.tracker.timeLine);
            
        }
        if (event & CAR_CTRL_NO_FIND_LINE) // 进入线线一段时间后离开线
        {
            EVENT_CLEAR(CAR_CTRL_NO_FIND_LINE);
            debug_print("No Find Line : %d \r\n", CarCtrl.tracker.timeNoLine);
        }
        if (event & CAR_CTRL_TIME_EVENT) // 时间事件
        {
            EVENT_CLEAR(CAR_CTRL_TIME_EVENT);
            debug_print("Time Event , Cnt:%d \r\n", CarCtrl.keyPointCnt);
        }
        car_ctrl_task_event(event);
    }
}

static void _task_alert_close(const void * arg)
{
    CarCtrl.port->buzzer_write(0);
    CarCtrl.port->led_write(0, 0);
}

// 发出声光警报信号
static void _alert(uint16_t timeMs)
{
    CarCtrl.port->buzzer_write(1);
    CarCtrl.port->led_write(0, 1);
    stimer_task_oneshot(_task_alert_close, timeMs, 0, NULL);
}

/* Task1: A -> B */
void car_ctrl_task1_start(const void * arg)
{
    debug_print("Task1 start\r\n");
    CarCtrl.task = CAR_CTRL_TASK1;
    car_ctrl_start_rotate(CarCtrl.port->angle_read(), 60);
    CarCtrl.tracker_event_cmp_time_line = 1000; // 离开线1s后才判断是否进线
    CarCtrl.tracker_event_cmp_time_noline = 1000; // 进入线1s后才判断是否离开线
    CarCtrl.check_point_flag = 1;
    CarCtrl.taskTime = stimer_get_tick(); // 记录时间
    EVENT_CLEAR(CAR_CTRL_ALL_EVENTS); // 清空时间
    
}

/* Task2: A -> B -> C -> D -> A */
void car_ctrl_task2_start(const void * arg)
{
    // debug_print("Task2 start\r\n");
    // CarCtrl.task = CAR_CTRL_TASK2;
    // car_ctrl_start_rotate(CarCtrl.port->angle_read(), 60);
    // CarCtrl.tracker_event_cmp_time_line = 1000; // 离开线1s后才判断是否进线
    // CarCtrl.tracker_event_cmp_time_noline = 1000; // 进入线1s后才判断是否离开线
    // CarCtrl.check_point_flag = 1;
    // CarCtrl.taskTime = stimer_get_tick(); // 记录时间
    // EVENT_CLEAR(CAR_CTRL_ALL_EVENTS); // 清空时间
    car_ctrl_auto_start(gtask2, sizeof(gtask2)/sizeof(gtask2[0]));
}

void car_ctrl_task3_start(const void * arg)
{
    car_ctrl_auto_start(gtask3, sizeof(gtask3)/sizeof(gtask3[0]));
}


void car_ctrl_task4_start(const void * arg)
{
    // 复制路径点，重复3次
    for (size_t i = 1; i < 4; i++)
    {
        memcpy(&gtask4[6*i], &gtask4[0], 6*sizeof(gtask4[0]));
    }
    car_ctrl_auto_start(gtask4, sizeof(gtask4)/sizeof(gtask4[0]));
}
/**
 * @brief 执行定点任务
 * @param keyPoint 
 * @param keyPointSize 
 * @param count 执行次数
 */
void car_ctrl_auto_start(CarKeyPoint_t *keyPoint, uint8_t keyPointSize)
{
    if (keyPoint == 0 || keyPointSize == NULL)
    {
        return;
    }
    debug_print("Auto start\r\n");
    CarCtrl.keyPoint = keyPoint;
    CarCtrl.keyPointSize = keyPointSize;
    CarCtrl.keyPointCnt = 1;
    CarCtrl.task = CAR_CTRL_AUTO;
    CarCtrl.tracker_event_cmp_time_line = 1000; // 离开线1s后才判断是否进线
    CarCtrl.tracker_event_cmp_time_noline = 1000; // 进入线1s后才判断是否离开线
    CarCtrl.check_point_flag = 1;
    CarCtrl.taskTime = stimer_get_tick(); // 记录时间
    CarCtrl.startAngle = CarCtrl.port->angle_read();
    EVENT_CLEAR(CAR_CTRL_ALL_EVENTS); // 清空时间
    car_ctrl_do_keypoint(&CarCtrl.keyPoint[0]);
    
}

void car_ctrl_task_start(Car_ctrl_task_e taskID)
{
    switch (taskID)
    {
    case CAR_CTRL_TASK1:
        stimer_task_oneshot(car_ctrl_task1_start, 1000, 1, NULL);
        break;
    case CAR_CTRL_TASK2:
        stimer_task_oneshot(car_ctrl_task2_start, 1000, 1, NULL);
        break;
    case CAR_CTRL_TASK3:
        stimer_task_oneshot(car_ctrl_task3_start, 1000, 1, NULL);
        break;
    case CAR_CTRL_TASK4:
        stimer_task_oneshot(car_ctrl_task4_start, 1000, 1, NULL);
        break;
    case CAR_CTRL_TEST:
        break;
    case CAR_CTRL_AUTO:
        
        break;
    }
}


void car_ctrl_do_keypoint(CarKeyPoint_t *keyPoint)
{
    switch (keyPoint->type)
    {
    case KEYPOINT_TYPE_STOP:
        car_ctrl_stop();
        break;
    case KEYPOINT_TYPE_LINE: // 走直线
        car_ctrl_start_rotate(CarCtrl.port->angle_read(), keyPoint->speed);
        break;
    case KEYPOINT_TYPE_ROTATE: // 转弯直走
        car_ctrl_start_rotate(CarCtrl.startAngle + keyPoint->angle, keyPoint->speed);
        break;
    case KEYPOINT_TYPE_TRACK: // 循迹
        car_ctrl_start_track(keyPoint->speed);
        break;
    case KEYPOINT_TYPE_ROTATEOPEN:
        car_ctrl_rotate_open(keyPoint->time, keyPoint->speed);
        CarCtrl.delayTime = keyPoint->time + 200; // 等待稳定
        break;
    default:
        break;
    }
}
/*-----------------------------------------------------------------------
|                             User Code                                 |
-----------------------------------------------------------------------*/

CarKeyPoint_t gtask2[TASK2_SIZE] = {
    {
        .type = KEYPOINT_TYPE_LINE,// A->B
        .event = 0,
        .speed = 30,
        .angle = 0,
        .distance = 100,
        .time = 2000,
    },
    {
        .type = KEYPOINT_TYPE_TRACK,// B->C
        .event = CAR_CTRL_FIND_LINE,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 0,
    },
    {
        .type = KEYPOINT_TYPE_ROTATEOPEN,// C点需要矫正角度
        .event = CAR_CTRL_NO_FIND_LINE,
        .speed = 40,
        .angle = 0,
        .distance = 0,
        .time = 100,
    },
    {
        .type = KEYPOINT_TYPE_LINE,// C->D
        .event = CAR_CTRL_TIME_EVENT,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 0,
    },
    {
        .type = KEYPOINT_TYPE_TRACK,// D->A
        .event = CAR_CTRL_FIND_LINE,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 0,
    },
    {
        .type = KEYPOINT_TYPE_STOP,// A end
        .event = CAR_CTRL_NO_FIND_LINE,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 0,
    }
};

// A -> C -> B -> D -> A
CarKeyPoint_t gtask3[TASK3_SIZE] = {
    {
        .type = KEYPOINT_TYPE_LINE,// A->C
        .event = 0,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 2000,
    },
    {
        .type = KEYPOINT_TYPE_TRACK,// C->B
        .event = CAR_CTRL_FIND_LINE,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 0,
    },
    {
        .type = KEYPOINT_TYPE_ROTATEOPEN,// B点需要矫正角度
        .event = CAR_CTRL_NO_FIND_LINE,
        .speed = -40,
        .angle = 0,
        .distance = 0,
        .time = 230,
    },
    {
        .type = KEYPOINT_TYPE_LINE,// B->D
        .event = CAR_CTRL_TIME_EVENT,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 0,
    },
    {
        .type = KEYPOINT_TYPE_TRACK,// D->A
        .event = CAR_CTRL_FIND_LINE,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 0,
    },
    {
        .type = KEYPOINT_TYPE_STOP,// A end
        .event = CAR_CTRL_NO_FIND_LINE,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 0,
    }
};


// A -> C -> B -> D -> |A -> C -> B -> D -> |A -> C -> B -> D -> |A -> C -> B -> D -> |A  
CarKeyPoint_t gtask4[TASK4_SIZE] = {
    {
        .type = KEYPOINT_TYPE_LINE,// A->C
        .event = CAR_CTRL_TIME_EVENT,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 2000,
    },
    {
        .type = KEYPOINT_TYPE_TRACK,// C->B
        .event = CAR_CTRL_FIND_LINE,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 0,
    },
    {
        .type = KEYPOINT_TYPE_ROTATEOPEN,// B点需要矫正角度
        .event = CAR_CTRL_NO_FIND_LINE,
        .speed = -40,
        .angle = 0,
        .distance = 0,
        .time = 240,
    },
    {
        .type = KEYPOINT_TYPE_LINE,// B->D
        .event = CAR_CTRL_TIME_EVENT,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 0,
    },
    {
        .type = KEYPOINT_TYPE_TRACK,// D->A
        .event = CAR_CTRL_FIND_LINE,
        .speed = 30,
        .angle = 0,
        .distance = 0,
        .time = 0,
    },
    {
    .type = KEYPOINT_TYPE_ROTATEOPEN,// A点需要矫正角度
    .event = CAR_CTRL_NO_FIND_LINE,
    .speed = 40,
    .angle = 0,
    .distance = 0,
    .time = 240,
    },
};

void car_ctrl_task_event(uint32_t events)
{
   // 进入模式
    switch (CarCtrl.task)
    {
    case CAR_CTRL_TEST:

    case CAR_CTRL_AUTO:
        if (CarCtrl.keyPointCnt < CarCtrl.keyPointSize) // 关键点未执行完
        {
            if (CarCtrl.keyPoint[CarCtrl.keyPointCnt].event & events) // 满足触发条件
            {
                _alert(1000);
                debug_print("KeyPoint cnt : %d start\r\n", CarCtrl.keyPointCnt);
                car_ctrl_do_keypoint(&CarCtrl.keyPoint[CarCtrl.keyPointCnt++]); // 执行下一个关键点
            }
        }
        else
        {
            CarCtrl.taskTime = stimer_get_tick() - CarCtrl.taskTime; 
            debug_print("Task Auto time : %d Ms\r\n", CarCtrl.taskTime);
            CarCtrl.task = CAR_CTRL_TASK_IDLE;
            car_ctrl_stop();
        }
        break;
    case CAR_CTRL_TASK1:
        if (events & CAR_CTRL_FIND_LINE && stimer_get_tick() - CarCtrl.taskTime > 1000)
        {
            CarCtrl.check_point_flag = 0;
            car_ctrl_stop();
            _alert(2000);
            CarCtrl.taskTime = stimer_get_tick() - CarCtrl.taskTime; 
            debug_print("Task1 time : %d Ms\r\n", CarCtrl.taskTime);
            CarCtrl.task = CAR_CTRL_TASK_IDLE;
        }
        break;
    case CAR_CTRL_TASK2:

        break;
    case CAR_CTRL_TASK3:

        break;
    case CAR_CTRL_TASK4:

        break;
    default:

        break;
    }
}


// 需要调整的参数
void car_task2_config_C(uint16_t time, int8_t speed)
{
    gtask2[2].time = time;
    gtask2[2].speed = speed;
}
void car_task3_config_B(uint16_t time, int8_t speed)
{
    gtask3[2].time = time;
    gtask3[2].speed = speed;
}
void car_task4_config_AB(int8_t speedAll, uint16_t time1, int8_t speed1, uint16_t time2, int8_t speed2)
{
    for (size_t i = 0; i < TASK4_SIZE; i++)
    {
        gtask4[i].speed = speedAll;
    }
    gtask4[2].time = time1;
    gtask4[2].speed = speed1;
    gtask4[5].time = time2;
    gtask4[5].speed = speed2;
    
}


